//
// Created by jg on 2021/3/5.
//

#ifndef PATH_TEST_PATH_TEST_H
#define PATH_TEST_PATH_TEST_H

#include <iostream>
#include <fstream>
#include <eigen3/Eigen/Dense>
#include <memory>
#include <vector>
#include <algorithm>
#include <thread>

#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

#include <active_location_path/active_location_path.h>
#include "BLH2locpos.hpp"

using std::vector;
using std::shared_ptr;
using std::thread;

namespace al_topo_map{
    enum IS_USE_ROS{
        yes = 0, no
    };

    struct EDGE_BOUND{
        double xmin, xmax, ymin, ymax;

        EDGE_BOUND(double _xmin, double _xmax, double _ymin, double _ymax) :
        xmin(_xmin), xmax(_xmax), ymin(_ymin), ymax(_ymax){}
    };

    std::string fileA
    = std::string("/home/jg/catkin_ws/src/wmr_plan/path_plan/path_test/GNSS_data/A/HL_INSPROBE_25_GNSS.txt");

    std::string fileB
    = std::string("/home/jg/catkin_ws/src/wmr_plan/path_plan/path_test/GNSS_data/B/HL_INSPROBE_26_GNSS.txt");

    class MAP_NODE{

    public:
        MAP_NODE(){}
        MAP_NODE(Eigen::Vector3d BLH){pos_BLH_ = std::move(BLH);}
        MAP_NODE(Eigen::Vector3d BLH, double e_N, double e_E, double e_D){
            pos_BLH_ = std::move(BLH);

            e_N_ = e_N;
            e_E_ = e_E;
            e_D_ = e_D;
        }

        Eigen::Vector3d pos_BLH_;
        Eigen::Vector3d loc_pos_;

        double e_N_, e_E_, e_D_;

        double time_;

        void setERR(double _eN, double _eE, double _eD){
            e_N_ = _eN;
            e_E_ = _eE;
            e_D_ = _eD;
        }

        typedef shared_ptr<MAP_NODE> Ptr;
    };
    
    class POS_ERR{
    public:
        POS_ERR(){}
        POS_ERR(double _time, double _eN, double _eE, double _eD):
        time_(_time), e_N_(_eN), e_E_(_eE), e_D_(_eD){}

        double time_;
        double e_N_, e_E_, e_D_;

        typedef shared_ptr<POS_ERR> Ptr;
    };

    class TOPO_MAP{
    protected:
        BLH2locpos blh2Locpos_;

        vector<MAP_NODE::Ptr> node_vec_;
        vector<POS_ERR::Ptr>  err_vec_;
        vector<EDGE_BOUND>    edge_bounds_;
        vector<double>        vec_err_;
        vector<vector<MAP_NODE::Ptr>> edge_node_;

        vector<ros::Publisher> pubs_;

        static IS_USE_ROS is_use_ROS_;

    public:
        TOPO_MAP(){}

        void run();

        void readFile(std::string file_name);
        void readGNSSData(std::string file_name);
        void readPOSERR(std::string file_name);

        void classifyEdge(vector<MAP_NODE::Ptr>& list);

        double getSIGMA(vector<MAP_NODE::Ptr>& list, double& RMS_N, double& RMS_E, double& RMS_D);

        void rvizInit(ros::NodeHandle& nh);

        void makeMap(const vector<Eigen::Vector3d>& list, double resolution, const Eigen::Vector4d& color, int id);

        static void isUSEROS(bool is_use_ROS){
            is_use_ROS_ = is_use_ROS ? yes : no;
        }
        static bool useROS(){
            return is_use_ROS_ == yes;
        }
    protected:
        double getFirstNumber(std::string& line);
    };

    IS_USE_ROS TOPO_MAP::is_use_ROS_ = yes;
}


#endif //PATH_TEST_PATH_TEST_H
